/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2Island.h"
#include "b2Body.h"
#include "b2Fixture.h"
#include "b2World.h"
#include "Contacts/b2Contact.h"
#include "Contacts/b2ContactSolver.h"
#include "Joints/b2Joint.h"
#include "../Common/b2StackAllocator.h"

b2Island::b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity, b2StackAllocator* allocator, b2ContactListener* listener)
: m_bodyCapacity(bodyCapacity)
, m_contactCapacity(contactCapacity)
, m_jointCapacity(jointCapacity)
, m_bodyCount(0)
, m_contactCount(0)
, m_jointCount(0)
, m_allocator(allocator)
, m_listener(listener)
{
	m_bodies = (b2Body**) m_allocator->Allocate(m_bodyCapacity * sizeof(b2Body*));
	m_contacts = (b2Contact**) m_allocator->Allocate(m_contactCapacity * sizeof(b2Contact*));
	m_joints = (b2Joint**) m_allocator->Allocate(m_jointCapacity * sizeof(b2Joint*));

	m_velocities = (b2Velocity*) m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));
	m_positions = (b2Position*) m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));
}

b2Island::~b2Island()
{
	m_allocator->Free(m_positions);
	m_allocator->Free(m_velocities);
	m_allocator->Free(m_joints);
	m_allocator->Free(m_contacts);
	m_allocator->Free(m_bodies);
}

void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
{
	// Integrate velocities and apply damping.
	for(int32 i=0; i < m_bodyCount; ++i)
	{
		b2Body* b = m_bodies[i];
		if(b->IsStatic())
		{
			continue;
		}

		// Integrate Velocity
		b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
		b->m_angularVelocity += step.dt * b->m_invI * b->m_torque;

		// Reset force
		b->m_force.SetZero();
		b->m_torque = 0.0f;

		// Apply damping.	//XY NOT understand here.
		// ODE: dv/dt + c * v = 0
		// Solution: v(t) = v0 * exp(-c * t)
		// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
		// v2 = exp(-c * dt) * v1
		// Taylor expansion:
		// v2 = (1.0f - c * dt) * v1
		b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f);
		b->m_angularVelocity *= b2Clamp(1.0f - step.dt*b->m_angularDamping, 0.0f, 1.0f);
	}

	b2ContactSolver contactSolver(step, m_contacts, m_contactCount, m_allocator);

	// Initialize velocity constraints.
	contactSolver.InitVelocityConstraints(step);

	for(int32 i=0; i<m_jointCount; ++i)
	{
		m_joints[i]->InitVelocityConstrains(step);
	}

	// Solve Velocity Constraint.
	for(int32 i=0; i<step.velocityIterations; ++i)
	{
		for(int32 j=0; j<m_jointCount; ++j)
		{
			m_joints[j]->SolveVelocityConstrains(step);
		}
		contactSolver.SolveVelocityConstraints();
	}

	// Post-solve (Store impulse for warm starting.)
	contactSolver.FinalizeVelocityConstraints();

	// Integrate Positions.
	for(int32 i=0; i<m_bodyCount; ++i)
	{
		b2Body* b = m_bodies[i];
		if(b->IsStatic())
		{
			continue;
		}

		// Check for large velocity.
		b2Vec2 translation = step.dt * b->m_linearVelocity;
		if(b2Dot(translation, translation) > b2_maxTranslationSquared)
		{
			translation.Normalize();
			b->m_linearVelocity = (b2_maxTranslation * step.inv_dt) * translation;
		}

		float32 rotation = step.dt * b->m_angularVelocity;
		if(rotation * rotation > b2_maxRotationSquared)
		{
			if(rotation < 0.0f)
			{
				b->m_angularVelocity = -step.inv_dt * b2_maxRotation;
			}
			else
			{
				b->m_angularVelocity = step.inv_dt * b2_maxRotation;
			}
		}

		// Store positions for continues collision.
		b->m_sweep.c0 = b->m_sweep.c;
		b->m_sweep.a0 = b->m_sweep.a;

		// Integrate.
		b->m_sweep.c += step.dt * b->m_linearVelocity;
		b->m_sweep.a += step.dt * b->m_angularVelocity;

		// Compute new transform.
		b->SynchronizeTransform();

		// Note: shapes are synchron later.
	}

	// Iterate over constraints
	for(int32 i=0; i < step.positionIterations; ++i)
	{
		bool contactsOk = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);

		bool jointsOk = true;
		for(int32 j=0; j < m_jointCount; ++j)
		{
			bool jointOk = m_joints[j]->SolvePositionConstrains(b2_contactBaumgarte);
			jointsOk = jointsOk && jointOk;
		}

		if(contactsOk && jointsOk)
		{
			// Exit early if the position errors are small.
			break;
		}
	}

	Report(contactSolver.m_constraints);

	if(allowSleep)
	{
		float32 minSleepTime = B2_FLT_MAX;
#ifndef TARGET_FLOAT32_IS_FIXED
		const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
		const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
#endif
		for(int32 i=0; i<m_bodyCount; ++i)
		{
			b2Body* b = m_bodies[i];
			if(b->m_invMass == 0.0f)
			{
				continue;
			}

			if ((b->m_flags & b2Body::e_allowSleepFlag) == 0 ||
#ifdef TARGET_FLOAT32_IS_FIXED
				b2Abs(b->m_angularVelocity) > b2_angularSleepTolerance ||
				b2Abs(b->m_linearVelocity.x) > b2_linearSleepTolerance ||
				b2Abs(b->m_linearVelocity.y) > b2_linearSleepTolerance)
#else
				b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
				b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
#endif
			{
				b->m_sleepTime = 0.0f;
				minSleepTime = 0.0f;
			}
			else
			{
				b->m_sleepTime += step.dt;
				minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
			}
		}

		if(minSleepTime >= b2_timeToSleep)
		{
			for(int32 i=0; i < m_bodyCount; ++i)
			{
				b2Body* b = m_bodies[i];
				b->m_flags |= b2Body::e_sleepFlag;
				b->m_linearVelocity = b2Vec2_zero;
				b->m_angularVelocity = 0.0f;
			}
		}
	}
}

void b2Island::SolveTOI(b2TimeStep& subStep)
{
	b2ContactSolver contactSolver(subStep, m_contacts, m_contactCount, m_allocator);

	// No warm starting is needed for TOI events because warm starting impulses were applied in the discrete solver.
	
	// Warm starting for joints is off for now, but we need to call this function to compute Jacobians.
	for(int32 i=0; i<m_bodyCount; ++i)
	{
		m_joints[i]->InitVelocityConstrains(subStep);
	}

	// Solve the velocity constraints.
	for(int32 i=0; i < subStep.velocityIterations; ++i)
	{
		contactSolver.SolveVelocityConstraints();
		for(int32 j=0; j<m_jointCount; ++j)
		{
			m_joints[j]->SolveVelocityConstrains(subStep);
		}
	}

	// Don't store the TOI contact forces for warm starting because they can be quite large.
	
	// Integrate positions.
	for(int32 i=0; i < m_bodyCount; ++i)
	{
		b2Body* b = m_bodies[i];
		if(b->IsStatic())
		{
			continue;
		}

		// Check for large velocities.
		b2Vec2 translation = subStep.dt * b->m_linearVelocity;
		if(b2Dot(translation, translation) > b2_maxTranslationSquared)
		{
			translation.Normalize();
			b->m_linearVelocity = (b2_maxTranslation * subStep.inv_dt) * translation;
		}

		float32 rotation = subStep.dt * b->m_angularVelocity;
		if(rotation * rotation > b2_maxRotationSquared)
		{
			if(rotation < 0.0f)
			{
				b->m_angularVelocity = - subStep.inv_dt * b2_maxRotation;
			}
			else
			{
				b->m_angularVelocity = subStep.inv_dt * b2_maxRotation;
			}
		}

		// Store positions for continous collision.
		b->m_sweep.c0 = b->m_sweep.c;
		b->m_sweep.a0 = b->m_sweep.a;

		// Integrate
		b->m_sweep.c += subStep.dt * b->m_linearVelocity;
		b->m_sweep.a += subStep.dt * b->m_angularVelocity;

		// Compute new transform
		b->SynchronizeTransform();

		// Note: shapes are synchronized later.
	}

#if 0
	{
		for (int32 i = 0; i < m_contactCount; ++i)
		{
			m_contacts[i]->Update(NULL);
		}
		b2ContactSolver contactSolver2(subStep, m_contacts, m_contactCount, m_allocator);

		// Solve position constraints.
		const float32 k_toiBaumgarte = 0.75f;
		for (int32 i = 0; i < subStep.positionIterations; ++i)
		{
			bool contactsOkay = contactSolver2.SolvePositionConstraints(k_toiBaumgarte);

			if (contactsOkay)
			{
				break;
			}
		}
	}
#else

	// Solve position constraints.
	const float32 k_toiBaumgarte = 0.75f;
	for (int32 i = 0; i < subStep.positionIterations; ++i)
	{
		bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte);
		bool jointsOkay = true;
		for (int32 j = 0; j < m_jointCount; ++j)
		{
			bool jointOkay = m_joints[j]->SolvePositionConstrains(k_toiBaumgarte);
			jointsOkay = jointsOkay && jointOkay;
		}

		if (contactsOkay && jointsOkay)
		{
			break;
		}
	}
#endif

	Report(contactSolver.m_constraints);
}

void b2Island::Report(const b2ContactConstraint *constraints)
{
	if(NULL == m_listener)
	{
		return;
	}

	for(int32 i=0; i<m_contactCount; ++i)
	{
		b2Contact* c = m_contacts[i];
		const b2ContactConstraint* cc = constraints + i;

		b2ContactImpulse impulse;
		for(int32 j=0; j<cc->pointCount; ++j)
		{
			impulse.normalImpulses[i] = cc->points[j].normalImpulse;
			impulse.tangentImpulses[i] = cc->points[j].tangentImpulse;
		}

		m_listener->PostSolve(c, &impulse);
	}
}
